/***************************************************************************
 *  Filename: maincontroller.cpp
 *  Class: MainController
 *
 *  Copyright 2012  Mario Henrique Voorsluys
 *  <forcaeluz@gmail.com>
 *
 ****************************************************************************/


#include "maincontroller.h"

MainController::MainController(bool gps, QObject *parent) :
    QObject(parent)
{
    connect(&timer, SIGNAL(timeout()), this, SLOT(controlLoop()));
    mutex.lock();
    roll = 0;
    pitch = 0;
    yaw = 0;
    vx = 0;
    vy = 0;
    vz = 0;
    targetX = 0;
    targetY = 0;
    targetZ = 0;
    x = 0;
    y = 0;
    z = 0;
    latitude = 0;
    longitude = 0;
    gpsMode = gps;
    mutex.unlock();
    timer.start(30);
}

void MainController::updateAnglesData(float troll, float tpitch, float tyaw){
    mutex.lock();
    roll = troll;
    pitch = tpitch;
    yaw = tyaw;
    mutex.unlock();
}

void MainController::updatePosition(float xt, float yt, float zt){
    mutex.lock();
    x = xt;
    y = yt;
    z = zt;
    mutex.unlock();
}

void MainController::updateSpeeds(float vxt, float vyt, float vzt, float tyawrate){
    mutex.lock();
    vx = vxt;
    vy = vyt;
    vz = vzt;
    yawRate = tyawrate;
    mutex.unlock();
}

void MainController::updateRelativeTarget(float xt, float yt, float zt, float yawt){
    mutex.lock();
    targetX = xt;
    targetY = yt;
    targetZ = zt;
    targetYaw = yawt;
    mutex.unlock();
}

void MainController::controlLoop(){
    if(!gpsMode){
        mutex.lock();
        float tz = z;

        float troll = roll;
        float tyaw = yaw;
        float tpitch = pitch;

        float tVx = vx;
        float tVy = vy;
        float tVz = vz;

        float tTx = targetX;
        float tTy = targetY;
        float tTz = targetZ;
        float tTyaw = targetYaw;
        mutex.unlock();

        float desiredPitch = (((tTx)*PropPitch - tVx*DiffVelx)-tpitch*DiffPitch)/MaxPitch;
        float desiredRoll = (((tTy)*PropPitch - tVy*DiffVely)-troll*DiffRoll)/MaxRoll;
        float desiredVz = (tTz - (tz*PropGaz + tVz*DiffGaz))/MaxGaz;
        float desiredYawRate = (tTyaw - tyaw*PropYaw)/MaxYaw;
        emit(updateCommands(desiredRoll, desiredPitch, desiredYawRate, desiredVz));
    }else{
        //TODO: Implement global positioning. Right now, keep hovering.

    }

}

void MainController::updateGpsInformation(float latitude, float longitude, float altitude, float bearing){

}

void MainController::updateGpsTarget(float latitude, float longitude, float altitude, float bearing){

}
